#ifndef _VISION_H
#define _VISION_H

#include "base_drv/drv_conf.h"
#include HAL_INCLUDE


extern float aim_spin_status;
extern float armor_theta_diff;
typedef __packed struct
{
    uint8_t header; // 0x5A
    uint8_t local_color : 1;
    uint8_t mode : 1;
    uint8_t reserved : 6;
    float roll;
    float pitch;
    float yaw; // 主要需要两个轴的角度数据
    float aim_x;
    float aim_y;
    float aim_z; // aim的三个参数主要可以提供给视觉调试时使用，可不发送数据
    uint16_t checksum;
} vision_req_t;

typedef __packed struct
{
    uint8_t header; // A5
    uint8_t target_found : 1;
    uint8_t id_num : 3;
    uint8_t armor_num : 3;
    uint8_t reserved : 1;
    float x, y, z;    // 在世界坐标系（imu的坐标系中的位置），z的值为当前瞄准装甲板的高度
    float yaw;        // 当前瞄准的装甲板朝向（在imu坐标系中的角度）
    float vx, vy, vz; // 三个轴的速度，预测用
    float v_yaw;      // 小陀螺速度
    float r1;         // 当前瞄准的装甲板与车身中心的水平距离
    float r2;         // 另一侧装甲板与车身中心的水平距离
    float dz;         // 另一侧装甲板的高度
    uint16_t checksum;
} vision_ctrl_t;

#define VISION_INF_USB 1

#define VISION_UART_REQ_SOF 0x5A
#define VISION_UART_CTRL_SOF 0xA5
#define VISION_CTRL_FRAME_LEN (sizeof(vision_ctrl_t))

	
#define VISION_DATA_NOERR 0x00
#define VISION_DATA_ERR 0xFF
#define VISION_NOTARGET 0xFE
#define VISION_OK 0x00

#define VISION_RX_LOST_MAX 30



enum e_ctrl_cmd {
    NONARMOR = 0xAA,
    GET_ANGLE = 0xBB,
    REQUEST_TRANS = 0xCC,
    NONARMOR_SLOW = 0xDD
};

enum e_buff_status {
    STATE_NONE = 0xEE,
    STATE_SMALL_BUF = 0xAA,
    STATE_LARGE_BUF = 0xBB
};

enum e_task_mode {
    MODE_AUTO_AIM = 0,
    MODE_SMALL_BUFF = 1,
    MODE_LARGE_BUFF = 2
};

enum e_shoot_mode {
    NO_FIRE = 0xDD,
    FIRE = 0xFF
};

extern vision_ctrl_t vision_ctrl_data;

#if VISION_INF_USB == 0
#define VISION_UART_HANDLE huart1
#define VISION_UART USART1

void vision_uart_idle_handler(UART_HandleTypeDef* huart);
HAL_StatusTypeDef vision_uart_dma_recv_init(void);

#else
uint8_t parse_vision_data(uint8_t* buf, uint16_t len);

#endif


void set_task_mode(uint8_t tm);
uint8_t get_vision_cmd(void);
//void set_vision_req_ang(float pitch, float yaw);
uint8_t vision_ctrl(float* pitch_ang, float* yaw_ang, float dt);

void send_vision_request(float current_pitch, float current_yaw);


uint32_t is_vision_offline(void);
void inc_vision_rx_lost(void);

uint8_t get_armour_state(void);
uint8_t is_vision_req(void);
void update_cur_v0(void);

#endif


